#include <stdint.h>
#include <string.h>

struct SerialArm_joints
{
    int state;                // 0: 0123mode, 1: 456mode
    float joint0_or_reserved; // -pi ~ pi
    float joint14;
    float joint25;
    float joint36;
};

struct SerialArm_joints_formated
{
    int8_t state; // 0: 0123mode, 1: 456mode
    int8_t joint0_or_reserved;
    int16_t joint14;
    int16_t joint25;
    int16_t joint36;
}; // 8 bytes

// #######################################

struct SerialArm_pose
{
    float x;     // -700 ~ 700 （mm）
    float y;     // -700 ~ 700 （mm）
    float z;     // -700 ~ 700 （mm）
    float roll;  // -pi ~ pi
    float pitch; // -pi ~ pi
    float yaw;   // -pi ~ pi
};

struct SerialArm_pose_formated
{
    int8_t x;
    int8_t y;
    int8_t z;
    int8_t roll;
    int16_t pitch;
    int16_t yaw;
}; // 8 bytes

#define PI 3.14159265358979323846
// #define INT8_MAX 0x7F
// #define INT16_MAX 0x7FFF
#define CLIP_IN_RANGE(value, max) ((value) <= (-max) ? (-max) : ((value) >= (max) ? (max) : (value)))
#define CLIP_AND_MAP_TO_RANGE(value, old_max, new_max) \
    ((value) <= -(old_max) ? -(new_max) : ((value) >= (old_max) ? (new_max) : ((value) * (new_max) / (old_max))))

void SerialArm_joints_encode(SerialArm_joints input, int8_t *data_formated)
{
    SerialArm_joints_formated data;
    data.state = input.state;
    data.joint0_or_reserved = (int8_t)CLIP_AND_MAP_TO_RANGE(input.joint0_or_reserved, PI, INT8_MAX);
    data.joint14 = (int16_t)CLIP_AND_MAP_TO_RANGE(input.joint14, PI, INT16_MAX);
    data.joint25 = (int16_t)CLIP_AND_MAP_TO_RANGE(input.joint25, PI, INT16_MAX);
    data.joint36 = (int16_t)CLIP_AND_MAP_TO_RANGE(input.joint36, PI, INT16_MAX);

    memcpy(data_formated, &data, sizeof(SerialArm_joints_formated));
}

void SerialArm_joints_decode(int8_t *data_formated, SerialArm_joints *output)
{
    SerialArm_joints_formated data;
    memcpy(&data, data_formated, sizeof(SerialArm_joints_formated));

    output->state = data.state;
    output->joint0_or_reserved = (float)data.joint0_or_reserved * PI / INT8_MAX;
    output->joint14 = ((float)data.joint14 / INT16_MAX) * PI ;
    output->joint25 = ((float)data.joint25 / INT16_MAX) * PI ;
    output->joint36 = ((float)data.joint36 / INT16_MAX) * PI ;
}


void SerialArm_pose_encode(SerialArm_pose input, int8_t *data_formated)
{
    SerialArm_pose_formated data;
    data.x = (int8_t)CLIP_AND_MAP_TO_RANGE(input.x, 700.0f, INT8_MAX);
    data.y = (int8_t)CLIP_AND_MAP_TO_RANGE(input.y, 700.0f, INT8_MAX);
    data.z = (int8_t)CLIP_AND_MAP_TO_RANGE(input.z, 700.0f, INT8_MAX);
    data.roll = (int8_t)CLIP_AND_MAP_TO_RANGE(input.roll, PI, INT8_MAX);
    data.pitch = (int16_t)CLIP_AND_MAP_TO_RANGE(input.pitch, PI, INT16_MAX);
    data.yaw = (int16_t)CLIP_AND_MAP_TO_RANGE(input.yaw, PI, INT16_MAX);

    memcpy(data_formated, &data, sizeof(SerialArm_pose_formated));
}

void SerialArm_pose_decode(int8_t *data_formated, SerialArm_pose *output)
{
    SerialArm_pose_formated data;
    memcpy(&data, data_formated, sizeof(SerialArm_pose_formated));

    output->x = (float)data.x * 700.0f / INT8_MAX;
    output->y = (float)data.y * 700.0f / INT8_MAX;
    output->z = (float)data.z * 700.0f / INT8_MAX;
    output->roll = (float)data.roll * PI / INT8_MAX;
    output->pitch = (float)data.pitch * PI / INT16_MAX;
    output->yaw = (float)data.yaw * PI / INT16_MAX;
}